#include <ctime>
#include <cstring>
#include <cmath>
#include <queue>
#include <vector>
#include <iostream>
#include <algorithm>
#include <climits>

using namespace std;

class Code01_AStarAlgorithm
{
public:
    // 0 : 上，1 : 右，2 : 下，3 : 左
    int move[5] = { -1, 0, 1, 0, -1 };

    // 曼哈顿距离
    int f1(int x, int y, int targetX, int targetY)
    {
        return abs(targetX - x) + abs(targetY - y);
    }

    // 对角线距离
    int f2(int x, int y, int targetX, int targetY)
    {
        return max(abs(targetX - x), abs(targetY - y));
    }

    // 欧式距离（勾股定理）
    double f3(int x, int y, int targetX, int targetY)
    {
        return sqrt(pow(targetX - x, 2) + pow(targetY - y, 2));
    }

    // Dijkstra 算法
	// grid[i][j] == 0 代表障碍
	// grid[i][j] == 1 代表道路
	// 只能走上、下、左、右，不包括斜线方向
	// 返回从 (startX, startY)  到(targetX, targetY) 的最短距离
    int minDistance1(vector<vector<int>>& grid, int startX, int startY, int targetX, int targetY)
    {
        // 起点或终点为 0，则无法到达，返回 -1
        if (grid[startX][startY] == 0 || grid[targetX][targetY] == 0) return -1;
        
        int m = grid.size(), n = grid[0].size();
        vector<vector<int>> dist(m, vector<int>(n, 0x3f3f3f3f));
        vector<vector<bool>> visited(m, vector<bool>(n, false));

        // 认为自己到自己是 1 的距离
        dist[startX][startY] = 1;
        // first : 从源点出发到达当前点的距离
        // second : pos，其中 pos = x * n + y，则 x = pos / n, y = pos % n
        // 将从源点出发到达当前点的距离作为 first 的目的是，能够复用标准库中 pair 的比较函数
        priority_queue<pair<int, int>, vector<pair<int, int>>, greater<pair<int, int>>> q;
        q.emplace(1, startX * n + startY);
        while(!q.empty())
        {
            auto [_, pos] = q.top();
            q.pop();
            int x = pos / n, y = pos % n;
            if(visited[x][y]) continue; 
            
            visited[x][y] = true;
            if(x == targetX && y == targetY) return dist[x][y];

            for(int i = 0; i < 4; ++i)
            {
                int nx = x + move[i], ny = y + move[i + 1];
                if(nx >= 0 && nx < m && ny >= 0 && ny < n
                        && grid[nx][ny] == 1
                        && !visited[nx][ny]
                        && dist[x][y] + 1 < dist[nx][ny])
                {
                    dist[nx][ny] = dist[x][y] + 1;
                    q.emplace(dist[nx][ny], nx * n + ny);
                }
            }
        }

        return -1; // 无法到达目标点
    }

    // A* 算法
	// grid[i][j] == 0 代表障碍
	// grid[i][j] == 1 代表道路
	// 只能走上、下、左、右，不包括斜线方向
	// 返回从 (startX, startY) 到 (targetX, targetY) 的最短距离
    int minDistance2(vector<vector<int>>& grid, int startX, int startY, int targetX, int targetY)
    {
        // 起点或终点为 0，则无法到达，返回 -1
        if (grid[startX][startY] == 0 || grid[targetX][targetY] == 0) return -1;

        int m = grid.size(), n = grid[0].size();
        vector<vector<int>> dist(m, vector<int>(n, 0x3f3f3f3f));
        vector<vector<bool>> visited(m, vector<bool>(n, false));

        // 认为自己到自己是 1 的距离
        dist[startX][startY] = 1;
        // first : 从源点出发到达当前点的距离 + 当前点到终点的预估距离
        // second : pos，其中 pos = x * n + y，则 x = pos / n, y = pos % n
        // 将从源点出发到达当前点的距离 + 当前点到终点的预估距离作为 first 的目的是，能够复用标准库中 pair 的比较函数
        priority_queue<pair<int, int>, vector<pair<int, int>>, greater<pair<int, int>>> q;
        q.emplace(1 + f1(startX, startY, targetX, targetY), startX * n + startY);
        while(!q.empty())
        {
            auto [_, pos] = q.top();
            q.pop();
            int x = pos / n, y = pos % n;
            if(visited[x][y]) continue;

            visited[x][y] = true;
            if(x == targetX && y == targetY) return dist[x][y];
            for(int i = 0; i < 4; ++i)
            {
                int nx = x + move[i], ny = y + move[i + 1];
                if(nx >= 0 && nx < m && ny >= 0 && ny < n
                        && grid[nx][ny] == 1
                        && !visited[nx][ny]
                        && dist[x][y] + 1 < dist[nx][ny])
                {
                    dist[nx][ny] = dist[x][y] + 1;
                    q.emplace(dist[nx][ny] + f1(nx, ny, targetX, targetY), nx * n + ny);
                }
            }
        }

        return -1; // 无法到达目标点
    }

    // 生成矩阵
    vector<vector<int>> randomGrid(int n)
    {
        vector<vector<int>> grid(n, vector<int>(n));
        for(int i = 0; i < n; ++i)
        {
            for(int j = 0; j < n; ++j)
            {
                int mod = rand() % 10; // [0, 9]
                if(mod >= 0 && mod <= 2) // 每个格子有 30% 的概率是 0
                    grid[i][j] = 0;
                else // 每个格子有 70% 的概率是 1
                    grid[i][j] = 1;
            }
        }
        return grid;
    }
};

// 为了测试
int main()
{
    srand(time(nullptr));
    int len = 100;
    int testTime = 1000;
    Code01_AStarAlgorithm obj;
    cout << "功能测试开始" << endl;
    for(int i = 0; i < testTime; ++i)
    {
        int n = len + rand() % 20;
        vector<vector<int>> grid = obj.randomGrid(n);
        int startX = rand() % n;
        int startY = rand() % n;
        int targetX = rand() % n;
        int targetY = rand() % n;
        int ans1 = obj.minDistance1(grid, startX, startY, targetX, targetY);
        int ans2 = obj.minDistance2(grid, startX, startY, targetX, targetY);
        if(ans1 != ans2) cout << "出错了！" << endl;
    }
    cout << "功能测试结束" << endl;

    cout << "性能测试开始" << endl;
    vector<vector<int>> grid = obj.randomGrid(4000);
    int startX = 0;
    int startY = 0;
    int targetX = 3900;
    int targetY = 3900;
    long start, end;
    start = clock();
    int ans1 = obj.minDistance1(grid, startX, startY, targetX, targetY);
    end = clock();
    cout << "运行 Dijskra 算法结果：" << ans1 << "，运行时间（毫秒）：" << end - start << endl; 

    start = clock();
    int ans2 = obj.minDistance2(grid, startX, startY, targetX, targetY);
    end = clock();
    cout << "运行 A* 算法结果：" << ans2 << "，运行时间（毫秒）：" << end - start << endl; 

    cout << "性能测试结束" << endl;

    return 0;
}